
/*
 *
 * Copyright (c) 2006 P.J.Leonard
 * 
 * http://www.frinika.com
 * 
 * This file is part of Frinika.
 * 
 * Frinika is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.

 * Frinika is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.

 * You should have received a copy of the GNU General Public License
 * along with Frinika; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
package uk.ac.bath.domains.IP;

import uk.ac.bath.base.MachineIF;
import uk.ac.bath.domains.IP.CartPole;

/**
 *
 * @author pjl
 */
public class CartSimQuad implements CartSimIF {

    int steps = 0, y;
    CartPole cp;
    MachineIF net;

    @Override
    public int getStepCount() {
        return steps;
    }

    public CartSimQuad(MachineIF net, float x, float x_dot, float theta, float theta_dot) { // boolean RANDOM_START) {
        this.net = net;
        steps = 0;
        cp = new CartPole(x, x_dot, theta, theta_dot);
        this.net = net;
    }

    public void tick() {

        steps++;

        /*-- setup the input layer based on the four iputs --*/


        net.setIn(0, (float) cp.getX());
        net.setIn(1, (float) cp.getXDot());

        double thet = cp.getPhi();
        double thetDot = cp.getPhiDot();

        double cosT = Math.cos(thet);
        double sinT = Math.sin(thet);


        double cosTDT = Math.cos(thetDot);
        double sinTDT = Math.sin(thetDot);

        net.setIn(2, (float) sinT);
        net.setIn(3, (float) sinTDT);


        net.setIn(4, (float) cosT);
        net.setIn(5, (float) cosTDT);


        net.setIn(4, 0.5f);
        net.tick();   /*-- activate the network based on the input --*/

        /*-- decide which way to push via which output unit is greater --*/
        if (net.out(0) > net.out(1)) {
            y = -1;
        } else {
            y = 1;
        }

        /*--- Apply action to the simulated cart-pole ---*/
        //        System.out.println(y+ " | " + cp );

        cp.tick(y);
    }

    public IPModelIF getModel() {
        return cp;
    }
}
